/*
 * Control: modbus
 * File: $Id: porttcpnetconn1.c   2020/06/05
 *
 *  Created on: 2020年06月05日
 *      Author: luwan
 */
#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>
#include "cmsis_os.h"


#include "lwip/api.h"
#include "lwip/netdb.h"
#include "lwip.h"

#include <lwip/sockets.h>
#include <lwip/err.h>
#include <lwip/sys.h>

#include "lwip/debug.h"
#include "lwip/stats.h"
#include "lwip/tcp.h"
#include "lwip/priv/tcp_priv.h"
/* ----------------------- Modbus includes ----------------------------------*/
#include "MBapp.h"
#include "common.h"

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
/* ----------------------- MBAP Header --------------------------------------*/
#define MB_TCP_UID 6
#define MB_TCP_LEN 4
#define MB_TCP_FUNC 7


/* ----------------------- Defines  -----------------------------------------*/
#define MB_TCP_DEFAULT_PORT 502  /* TCP listening port. */
#define MB_TCP_BUF_SIZE (256 + 7) /* Must hold a complete Modbus TCP frame. */


#define MB_HEAD_LEN   6 
#define MB_ADDR_SN   (0 + MB_HEAD_LEN) 
#define MB_CMD_SN   (1 + MB_HEAD_LEN)
#define MB_REG_SN   (2 + MB_HEAD_LEN)
#define MB_LEN_SN   (4 + MB_HEAD_LEN)
#define MB_DATA_SN  (7 + MB_HEAD_LEN)

#define MB_DEV_ADDR  1
/*
*********************************************************************************************************
*                                            LOCAL TABLES
*********************************************************************************************************
*/
static err_t MB_server_accept(void *arg, struct tcp_pcb *pcb, err_t err);
static err_t MB_server_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err);
static uint8_t  MB_DealRcvBuff(uint8_t *rcvbuff,uint16_t length);
/*
*********************************************************************************************************
*                                      LOCAL FUNCTION PROTOTYPES
*********************************************************************************************************
*/
static uint8_t  mbbuff[MB_TCP_BUF_SIZE] = {0};
static uint16_t mbSendLen;

/************************/
void MB_Server_Init(void)
{
        struct tcp_pcb *MB_server_pcb = NULL;

	      /* 为tcp分配tcp_pcb结构体 */
        MB_server_pcb = tcp_new();

        /* 初始化IP与端口 */
        tcp_bind(MB_server_pcb, IP_ADDR_ANY, MB_TCP_DEFAULT_PORT);

        /* 监听tcp_server_pcb */
        MB_server_pcb = tcp_listen(MB_server_pcb);

        /* 初始化接收回调函数 */
        tcp_accept(MB_server_pcb, MB_server_accept);
}


#if 0
/*******************只能开一个modbus断开***************************/
struct tcp_pcb *MB_send_pcb = NULL;
static err_t MB_server_accept(void *arg, struct tcp_pcb *pcb, err_t err)
{
        /* 确定监听与连接 */
	    if(MB_send_pcb != NULL){
				#ifdef MB_TCP_BUG    
				printf("MB_send_pcb: close\r\n");  
        #endif
        tcp_abort(MB_send_pcb);
				tcp_abort(pcb);
				MB_send_pcb = NULL;
				return ERR_OK;
			}
      tcp_arg(pcb,  pcb);//tcp_arg(pcb, mem_calloc(sizeof(struct name), 1));
      #ifdef MB_TCP_BUG
      printf("MBtcp: accept\r\n");     
	    #endif
      /* 接收回调与主动发送结构体赋值 */
	    MB_send_pcb = pcb;
	    /* 禁止小帧数据合并，否则会出现阻塞，内存溢出错误 */
      tcp_nagle_disable(MB_send_pcb);
      tcp_recv(pcb, MB_server_recv);

      return ERR_OK;
}
#else
/*******************最多能开两个modbus断开***************************/
struct tcp_pcb *MB_send_Muxpcb[2] = {NULL};
uint16_t MuxpcbIndex = 0;

void
TCPPortClose( struct tcp_pcb *pxPCB )
{
    if( pxPCB != NULL )
    {
        if( tcp_close( pxPCB ) != ERR_OK )
        {
            tcp_abort( pxPCB );
        }
    }
}
void tcp_find_waitPcb(void)
{
    struct tcp_pcb *pcb = NULL;

		for(pcb = tcp_active_pcbs; pcb != NULL; pcb = pcb->next)
		{
			
			/****多次网线断开连接，pcb指向函数地址0x080XXXX，释放这类PCB***/
			if(((uint32_t)pcb & 0x2ff00000) != 0x20000000){
				#ifdef MB_TCP_BUG
				printf("MBtcp: err  pcb  %0x\r\n",(uint32_t)pcb);
				#endif
				TCPPortClose( pcb );
			}
			if(pcb->state == FIN_WAIT_1 || pcb->state == FIN_WAIT_2 ){
				if(pcb == MB_send_Muxpcb[0]){
					MB_send_Muxpcb[0] = NULL;
				}
				else if(pcb == MB_send_Muxpcb[1]){
					MB_send_Muxpcb[1] = NULL;
				}
				tcp_abort( pcb );
				#ifdef MB_TCP_BUG
				printf("MBtcp: wait  pcb  %0x\r\n",(uint32_t)pcb);
				#endif
			}		
		}
}

static err_t MB_server_accept(void *arg, struct tcp_pcb *pcb, err_t err)
{
        /* 确定监听与连接 */
	    if(pcb == NULL){
				/*网络不稳（不断插拔网线），强制释放pcb*/
				tcp_find_waitPcb();
				#ifdef MB_TCP_BUG
				printf("MBtcp: accept  pcb == NULL\r\n");
				#endif
				return ERR_ARG;
			}
			if(((uint32_t)pcb & 0x2ff00000) != 0x20000000){
				#ifdef MB_TCP_BUG
				printf("MBtcp: accept  pcb ERR\r\n");
				#endif
				tcp_find_waitPcb();
				TCPPortClose( pcb );
				return ERR_OK;
			}
	    if((MB_send_Muxpcb[0] != NULL)&&(MB_send_Muxpcb[1] != NULL)){
	
				MuxpcbIndex = (MuxpcbIndex>=2) ? 0 : MuxpcbIndex;     
        TCPPortClose(MB_send_Muxpcb[MuxpcbIndex]);
				MB_send_Muxpcb[MuxpcbIndex] = NULL;
				#ifdef MB_TCP_BUG
				printf("MB_send_pcb: close %d\r\n",MuxpcbIndex);
				#endif
//			  TCPPortClose(pcb);   屏蔽减少一次再连接2020-07-10
//				return ERR_OK;
			}
	    
			if(MB_send_Muxpcb[0] == NULL){
				#ifdef MB_TCP_BUG
				printf("MBtcp: accept  0   %0x\r\n",(uint32_t)pcb);  
				#endif
				MuxpcbIndex = 1;  			
				/* 接收回调与主动发送结构体赋值 */
	      MB_send_Muxpcb[0] = pcb;
			}
			else if(MB_send_Muxpcb[1] == NULL){
				#ifdef MB_TCP_BUG
				printf("MBtcp: accept  1   %0x\r\n",(uint32_t)pcb); 
				#endif
				MuxpcbIndex = 0; 		
				/* 接收回调与主动发送结构体赋值 */
	      MB_send_Muxpcb[1] = pcb;
			}
			else{
				#ifdef MB_TCP_BUG
				printf("MBtcp: accept  unkown\r\n");
				#endif
				return ERR_ARG;
			}
      tcp_arg(pcb,  pcb);       
	      /* 禁止小帧数据合并，否则会出现阻塞，内存溢出错误 */
      tcp_nagle_disable(pcb);
      tcp_recv(pcb, MB_server_recv);
			
      return ERR_OK;
}
#endif

/***接收回调函数**/
static err_t MB_server_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *MB_recv_pbuf, err_t err)
{
        struct pbuf *MB_send_pbuf;
        err_t tcp_err;
        if (MB_recv_pbuf != NULL)
        {
                /* 接收数据 */
                tcp_recved(pcb, MB_recv_pbuf->tot_len);

                /* 接收数据指针 */
                MB_send_pbuf = MB_recv_pbuf;
					
                /* 解析接收数据 */									
								MB_DealRcvBuff(MB_send_pbuf->payload, MB_send_pbuf->len);
                /* 将接收数据发送回去 */
                if(mbSendLen)
								{									
									tcp_err = tcp_write(pcb, mbbuff, mbSendLen, 1);
									if(tcp_err == ERR_OK) {
							       ( void )tcp_output( pcb );
							    }
									mbSendLen = 0;
								}
                pbuf_free(MB_recv_pbuf);
        }
        else if (err == ERR_OK)
        {
					  if(pcb == MB_send_Muxpcb[0]){
							MB_send_Muxpcb[0] = NULL;
						}
						else if(pcb == MB_send_Muxpcb[1]){
							MB_send_Muxpcb[1] = NULL;
						}						
            return tcp_close(pcb);
        }
        return ERR_OK;
}
static void  MB_ERR( uint8_t *rcvbuff,uint16_t length )
{	
	mbSendLen = 0;
	memcpy(mbbuff,rcvbuff,MB_HEAD_LEN);
	mbSendLen += MB_HEAD_LEN;
	mbbuff[mbSendLen++] = rcvbuff[MB_ADDR_SN];//设备地址
	mbbuff[mbSendLen++] = rcvbuff[MB_CMD_SN]+0x80;;//命令码
	mbbuff[mbSendLen++] = 1;
	mbbuff[MB_HEAD_LEN - 1] = 3;// modbus除头长度  	
}

/********************************8个线圈转成一个字节*********************************************/
static uint8_t MB_GetBits( uint8_t * ucByteBuf, uint16_t usBitOffset, uint8_t ucNBits )
{
    uint8_t     i;
    uint8_t   usBuf;
	
	  usBuf = 0;
    for(i=0;i<ucNBits;i++){
			if(ucByteBuf[usBitOffset+i])usBuf |= (1<<i);
		}
    return usBuf;
}
/********************************read funtion cmd =1********************************************/
static uint8_t  MB_ReadBit_Cmd01( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usbitAddress;
    int16_t   usBitCount;
	  uint16_t   usBitOffset,bitlen;
  
    usbitAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usbitAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usBitCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usBitCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);
   
	  bitlen = usBitCount/8;
	  bitlen = (usBitCount%8)?(bitlen+1):bitlen;
	   
	 
		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,MB_HEAD_LEN);
	  mbSendLen += MB_HEAD_LEN;
		if( ( usBitCount >= 1 ) && ( usBitCount + usbitAddress <= REG_COILS_NREGS ) )
		{
				mbbuff[mbSendLen++] = rcvbuff[MB_ADDR_SN];//设备地址
			  mbbuff[mbSendLen++] = rcvbuff[MB_CMD_SN];//命令码
				mbbuff[mbSendLen++] = ( uint8_t )bitlen;// 数据长度          			
			  mbbuff[MB_HEAD_LEN - 1] = ( uint8_t )bitlen + 3;// modbus除头长度   
				//
				usBitOffset = usbitAddress;
				while( usBitCount > 0 ){//
					mbbuff[mbSendLen++] = MB_GetBits(( uint8_t*)usRegCoilsBuf1, usBitOffset,( uint8_t)( usBitCount > 8 ? 8 : usBitCount ) );
					usBitCount -= 8;
					usBitOffset += 8;
				}
		} 
    else{
			MB_ERR(rcvbuff, length);
		}			
    return 0;
}


/********************************read funtion cmd =2*********************************************/
static uint8_t  MB_ReadBit_Cmd02( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usbitAddress;
    int16_t   usBitCount;
	  uint16_t   usBitOffset,bitlen;
	
    usbitAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usbitAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usBitCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usBitCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);
   
	  bitlen = usBitCount/8;
	  bitlen = (usBitCount%8)?(bitlen+1):bitlen;
	
		mbSendLen = 0;
  	memcpy(mbbuff,rcvbuff,MB_HEAD_LEN);
	  mbSendLen += MB_HEAD_LEN;
		if( ( usBitCount >= 1 ) && ( usBitCount + usbitAddress <= REG_DISCRETE_NREGS ) )
		{       
        mbbuff[mbSendLen++] = rcvbuff[MB_ADDR_SN];//设备地址
			  mbbuff[mbSendLen++] = rcvbuff[MB_CMD_SN];//命令码
				mbbuff[mbSendLen++] = ( uint8_t )bitlen;// 数据长度          			
			  mbbuff[MB_HEAD_LEN - 1] = ( uint8_t )bitlen + 3;// modbus除头长度   			
							
				usBitOffset = usbitAddress;
				while( usBitCount > 0 ){//
					mbbuff[mbSendLen++] = MB_GetBits(( uint8_t*)usRegDiscreteBuf2, usBitOffset,( uint8_t)( usBitCount > 8 ? 8 : usBitCount ) );
					usBitCount -= 8;
					usBitOffset += 8;
				}	
		} 
    else{
			MB_ERR(rcvbuff,length);
		}      
    return 0;
}
/*************************read funtion cmd =3***************************/
static uint8_t  MB_ReadReg_Cmd03( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress,usRegData;
    uint16_t   usRegCount;
	  uint16_t   i;

	
    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usRegCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usRegCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);

		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,MB_HEAD_LEN);
	  mbSendLen += MB_HEAD_LEN;
		if( ( usRegCount >= 1 ) && ( usRegCount + usRegAddress <= REG_HOLDING_NREGS ) )
		{  
        mbbuff[mbSendLen++] = rcvbuff[MB_ADDR_SN];//设备地址
			  mbbuff[mbSendLen++] = rcvbuff[MB_CMD_SN];//命令码
				mbbuff[mbSendLen++] = ( uint8_t ) ( usRegCount * 2 );// 数据长度          			
			  mbbuff[MB_HEAD_LEN - 1] = ( uint8_t )( usRegCount * 2 ) + 3;// modbus除头长度   			
				
		
				for(i=0;i<usRegCount;i++){//
					usRegData = usRegHoldingBuf3[usRegAddress+i].word;
					mbbuff[mbSendLen++] = ( uint8_t )( usRegData >> 8 );
					mbbuff[mbSendLen++] = ( uint8_t )( usRegData & 0xFF );
				}
								
		} 
    else{
			MB_ERR(rcvbuff,length); 
		}      
    return 0;
}

/*************************read funtion cmd =4***************************/
static uint8_t  MB_ReadReg_Cmd04( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress,usRegData;
    uint16_t   usRegCount;
	  uint16_t   i;
	
    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usRegCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usRegCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);


		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,MB_HEAD_LEN);
	  mbSendLen += MB_HEAD_LEN;
		if( ( usRegCount >= 1 ) && ( usRegCount + usRegAddress <= REG_INPUT_NREGS ) )
		{
				mbbuff[mbSendLen++] = rcvbuff[MB_ADDR_SN];//设备地址
			  mbbuff[mbSendLen++] = rcvbuff[MB_CMD_SN];//命令码
				mbbuff[mbSendLen++] = ( uint8_t ) ( usRegCount * 2 );// 数据长度          			
			  mbbuff[MB_HEAD_LEN - 1] = ( uint8_t )( usRegCount * 2 ) + 3;// modbus除头长度           
				
				for(i=0;i<usRegCount;i++){//
					usRegData = usRegInputBuf4[usRegAddress+i].word;
					mbbuff[mbSendLen++] = ( uint8_t )( usRegData >> 8 );
					mbbuff[mbSendLen++] = ( uint8_t )( usRegData & 0xFF );
				}    					
		} 
    else{
			MB_ERR(rcvbuff,length); 
		}      
    return 0;
}

/*********************write funtion cmd = 5  **************************/
static uint8_t  MB_WriteBit_Cmd05( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress;

    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
		
		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,(MB_HEAD_LEN+6));
	  mbSendLen += (MB_HEAD_LEN+6);
		  			 						
		if(rcvbuff[mbSendLen - 2]){
			usRegCoilsBuf1[usRegAddress]= 0x01;
		}
		else{
			usRegCoilsBuf1[usRegAddress]= 0x00;
		}				    					
    return 0;
}
/*********************write funtion cmd = 15  **************************/
static uint8_t  MB_WriteBit_Cmd0F( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress;
    uint16_t   usBitCount,bitlen,offer;
	  uint16_t   i,j;
	  uint8_t    byteMask;
	
    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usBitCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usBitCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);

    bitlen = usBitCount/8;
	  bitlen = (usBitCount%8)?(bitlen+1):bitlen;
	
		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,(MB_HEAD_LEN+6));
	  mbSendLen += (MB_HEAD_LEN+6);
		if( ( usBitCount >= 1 ) && ( usBitCount + usRegAddress <= REG_COILS_NREGS ) )
		{   			
			  mbbuff[MB_HEAD_LEN - 1] = 6;// modbus除头长度   			
				for(i=0;i<usBitCount;i++){//
					j = mbSendLen + 1 + i/8;
					offer = i%8;
					byteMask = 0x01<<offer;
					if(rcvbuff[j]&byteMask){
						usRegCoilsBuf1[usRegAddress+i]= 0x01;
					}
					else{
						usRegCoilsBuf1[usRegAddress+i]= 0x00;
					}
				}    					
		} 
    else{
			MB_ERR(rcvbuff,length);
		}      
    return 0;
}

/*********************write funtion cmd = 6  **************************/
static uint8_t  MB_WriteReg_Cmd06( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress;
    uint16_t   regdata;
	
    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	   
		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,(MB_HEAD_LEN+6));
	  mbSendLen += (MB_HEAD_LEN+6);

		regdata = (rcvbuff[mbSendLen - 2]<<8)+rcvbuff[mbSendLen - 1];
		usRegHoldingBuf3[usRegAddress].word = regdata;
				    				    
    return 0;
}
/*********************write funtion cmd = 16  **************************/
static uint8_t  MB_WriteReg_Cmd10( uint8_t * rcvbuff, uint16_t length )
{
    uint16_t   usRegAddress;
    uint16_t   usRegCount,regdata;
	  uint16_t   i,j;
	
    usRegAddress = ( uint16_t )(rcvbuff[MB_REG_SN]<<8);
	  usRegAddress |= ( uint16_t )(rcvbuff[MB_REG_SN+1]);
	
	  usRegCount = ( uint16_t )(rcvbuff[MB_LEN_SN]<<8);
	  usRegCount |= ( uint16_t )(rcvbuff[MB_LEN_SN+1]);
    
		mbSendLen = 0;
	  memcpy(mbbuff,rcvbuff,(MB_HEAD_LEN+6));
	  mbSendLen += (MB_HEAD_LEN+6);
		if( ( usRegCount >= 1 ) && ( usRegCount + usRegAddress <= REG_INPUT_NREGS ) )
		{
        mbbuff[MB_HEAD_LEN - 1] = 6;// modbus除头长度   	
				
				for(i=0;i<usRegCount;i++){//
					j = MB_DATA_SN + 2 * i;
					regdata = (rcvbuff[j]<<8)+rcvbuff[j+1];
					usRegHoldingBuf3[usRegAddress+i].word= regdata;
				}    			
		} 
    else{
			MB_ERR(rcvbuff,length);
		}      
    return 0;
}

/******************接收处理***********************/
static uint8_t  MB_DealRcvBuff(uint8_t *rcvbuff,uint16_t length)
{
  uint16_t islen;
	
	if((uint8_t)MB_DEV_ADDR != rcvbuff[MB_TCP_UID]){//设备ID
		MB_ERR(rcvbuff,length);
		return  0;
	}
	if(length>MB_TCP_BUF_SIZE){//长度
		MB_ERR(rcvbuff,length);
		return  0;	
	}
	islen = (rcvbuff[MB_TCP_LEN]<<8) + rcvbuff[MB_TCP_LEN+1] + MB_HEAD_LEN;
	if(length != islen){//长度
		MB_ERR(rcvbuff,length);
		return  0;	
	}

	switch(rcvbuff[MB_TCP_FUNC]){
		case 1:
			MB_ReadBit_Cmd01( rcvbuff,length);		
			break;
		case 2:
			MB_ReadBit_Cmd02( rcvbuff,length);
			break;
		case 3:
			MB_ReadReg_Cmd03( rcvbuff,length);
			break;
		case 4:
			MB_ReadReg_Cmd04( rcvbuff,length);
			break;
		case 0x05:
			MB_WriteBit_Cmd05( rcvbuff,length);//
			break;
		case 0x0f:
			MB_WriteBit_Cmd0F( rcvbuff,length);//
			break;
		case 0x06:
			MB_WriteReg_Cmd06( rcvbuff,length);//
			break;
		case 0x10:
			MB_WriteReg_Cmd10( rcvbuff,length);//
			break;
		default:			
		  MB_ERR(rcvbuff,length);		
			break;
	}	
	return 0;
}
